package ai.agents.astar;

import java.awt.Color;
import java.awt.Graphics;
import java.util.Stack;

import ai.application.Config;
import ai.application.Environment;
import ai.astar.DirectedGraph;
import ai.astar.Node;
import ai.gui.Drawable;
import ai.moving_objects.Changeable;
import ai.spatial.Vector2D;

public class PathFindingAgent implements Changeable, Drawable{

	private Vector2D position;
	Vector2D velocity;
	private DirectedGraph aStarGraph;
	private Stack<Node> path;
	
	public PathFindingAgent(){
		position = new Vector2D(0,0);
		velocity = new Vector2D(0,0);
	}
	
	@Override
	public void update(Environment environment) {
		if(!path.empty()){
			Node nextNode = path.peek();
			if(new Vector2D(nextNode.getX(), nextNode.getY()).substract(position).length() < 5){
				nextNode = path.pop();
			}
			velocity = new Vector2D(nextNode.getX(), nextNode.getY()).substract(position).normalize();
			position.setX(position.getX() + velocity.getX() * Config.getSimStepSize());
			position.setY(position.getY() + velocity.getY() * Config.getSimStepSize());
		}else{
			System.out.println("finish!");
		}
	}

	@Override
	public void draw(Graphics g) {
		int diameter = 15;
		g.setColor(new Color(119, 185, 0));
		g.fillOval((int)position.getX()-diameter/2, (int)position.getY()-diameter/2, diameter, diameter);
	}

	public void setGraph(DirectedGraph graph) {
		this.aStarGraph = graph;
	}

	public void findPath(String startNodeString, String stargetNodeString) {
		path = aStarGraph.aStar(startNodeString, stargetNodeString);
		if(!path.empty()){
			Node startNode = path.pop();
			position.setX(startNode.getX());
			position.setY(startNode.getY());
		}
//		int i=0;
//		for (Node node : path) {
//			System.out.println("i: "+i+" node: "+node.getId());
//			i++;
//		}
		
		if(!path.empty()){
			Node nextNode = path.peek();
			velocity = new Vector2D(nextNode.getX(), nextNode.getY()).substract(position).normalize();
		}
	}	
}
